FUNCTION_BLOCK LCB_TablePositioning
TITLE = 'LCB_TablePositioning'

// Functionblock for Lenze Application Tablepositioning with 8400 and 9400 Drives. 
// A special Application is needed on the Drive. See documentation for details.
// The implementation is independant of the used bussystem (Profinet / Profibus)
//
// Used S7 Systemfunctions 
// SFC14 (DPRD_DAT)
// SFC15 (DPWR_DAT)
//
// Versionshistory:
// 1.0 Ersterstellung E.Dittmann
// 1.1 Bugfix
// 2.0 Release


AUTHOR : 'Lenze'
Family : 'LCB'
Version : '2.0'
Name: 'TabPos'
KNOW_HOW_PROTECT


CONST
    Range84 := 4; // Count of wors from / to the drive 
    Range94 := 5; // Count of wors from / to the drive 
END_CONST

VAR_INPUT
    iFirstPeripherieADR     : INT;                                  // Peripheral adress of the drive (Hardwareconfig S7)
    AxisData                : BLOCK_DB;                             // AxisData DB-Nr.
    xEnableInternalControl  : BOOL := FALSE;                        // Enable internal drivecontrol with variabletable
    iDriveType              : INT := 0;                             // 2=8400 HL / TL, 11=9400 HL
    xDriveEnable            : BOOL := FALSE;                        // Enable Drive
    xDriveSetQsp            : BOOL := FALSE;                        // Set Quickstop
    xResetError             : BOOL := FALSE;                        // Reset Error
    xManualPos              : BOOL := FALSE;                        // Manual Positiv
    xManualNeg              : BOOL := FALSE;                        // Manual Negativ
    xHomingSet              : BOOL := FALSE;                        // Set Homeposition
    xHomingStart            : BOOL := FALSE;                        // Start Homing
    xHomingReset            : BOOL := FALSE;                        // Reset Homeposition
    xProfileStart           : BOOL := FALSE;                        // Start Positioning
    xProfileRestart         : BOOL := FALSE;                        // Restart Positioning
    xProfileReset           : BOOL := FALSE;                        // Reset Positioning
    xProfilePosStop         : BOOL := False;                        // Stop Positioning 
    wProfileNumberSet       : WORD := 1;                            // Profilnumber
    xEnableSpeedOverride    : BOOL := FALSE;                        // Enable Speedoverride
    rSpeedOverrideSet       : REAL := 100.0;                        // Overridevalue in %
    xEnablePositionTeach    : BOOL := FALSE;                        // Enable Positionteaching
    rPositionSet            : REAL := 0.0;                          // Targetposition
END_VAR

VAR_OUTPUT
    xInternalControlActive  : BOOL;                                 // Internal control active
    xError                  : BOOL;                                 // Error
    xCommunicationOK        : BOOL;                                 // Communication OK
    xDriveError             : BOOL;                                 // Drive Error
    xDriveWarning           : BOOL;                                 // Drive Warning
    xDriveReady             : BOOL;                                 // Drive Ready
    xDriveEnabled           : BOOL;                                 // Drive Enabled 
    xDriveQspActive         : BOOL;                                 // Quickstop active
    xSpeedEqZero            : BOOL;                                 // Speed equal zero
    xHomingActive           : BOOL;                                 // Homing active   
    xHomingDone             : BOOL;                                 // Homing done
    xHomePosAvailable       : BOOL;                                 // Homeposition available
    xManualActive           : BOOL;                                 // Manual active
    xProfileActive          : BOOL;                                 // Positioning active
    xProfileDone            : BOOL;                                 // Positioning done
    xProfileInTarget        : BOOL;                                 // Positioning in target
    xHW_LimitSwitchPos      : BOOL;                                 // Hardwarelimitswitch positiv
    xHW_LimitSwitchNeg      : BOOL;                                 // Hardwarelimitswitch negativ
    rSpeedActual            : REAL;                                 // Actual Speed
    rPosActual              : REAL;                                 // Actual Position
    iReadError              : INT;                                  // Errorcode SFC14 
    iWriteError             : INT;                                  // Errorcode SFC15 

END_VAR

VAR_IN_OUT

END_VAR


VAR
    wFirstPeripherieADR         : WORD;                             
    wProfileNumberSetIntern     : WORD;
    diActPos_p                  : DINT;
    rAccOverride                : REAL;
    xDriveEnable_IC             : BOOL := FALSE;
    xDriveSetQsp_IC             : BOOL := FALSE;
    xResetError_IC              : BOOL := FALSE;
    xManualPos_IC               : BOOL := FALSE;
    xManualNeg_IC               : BOOL := FALSE;
    xHomingSet_IC               : BOOL := FALSE;
    xHomingStart_IC             : BOOL := FALSE;
    xHomingReset_IC             : BOOL := FALSE;
    xProfileStart_IC            : BOOL := FALSE;
    xProfileRestart_IC          : BOOL := FALSE;
    xProfileReset_IC            : BOOL := FALSE;
    xProfilePosStop_IC          : BOOL := False;
    wProfileNumberSet_IC        : WORD := 1;
    xwInput AT wProfileNumberSet_IC: ARRAY[0..15] OF BOOL;

    xEnableSpeedOverride_IC     : BOOL := FALSE;
    rSpeedOverrideSet_IC        : REAL := 100.0;
    xEnablePositionTeach_IC     : BOOL := FALSE;
    
    rPositionSet_IC             : REAL := 0.0;        
    scInternalCtrl84:         STRUCT                                  
                                b8:     BOOL:= FALSE;
                                b9:     BOOL:= FALSE;
                                b10:    BOOL:= FALSE;
                                b11:    BOOL:= FALSE;
                                b12:    BOOL:= FALSE;
                                b13:    BOOL:= FALSE;
                                b14:    BOOL:= FALSE;
                                b15:    BOOL:= FALSE;
                                b0:     BOOL:= FALSE;
                                b1:     BOOL:= FALSE;
                                b2:     BOOL:= FALSE;
                                b3:     BOOL:= FALSE;
                                b4:     BOOL:= FALSE;
                                b5:     BOOL:= FALSE;
                                b6:     BOOL:= FALSE;
                                b7:     BOOL:= FALSE;
                                awInt:  ARRAY[2..Range84] OF WORD;
                            END_STRUCT;
                            
    scInternalState84:        STRUCT                                  
                                b8:     BOOL;
                                b9:     BOOL;
                                b10:    BOOL;
                                b11:    BOOL;
                                b12:    BOOL;
                                b13:    BOOL;
                                b14:    BOOL;
                                b15:    BOOL;
                                b0:     BOOL;
                                b1:     BOOL;
                                b2:     BOOL;
                                b3:     BOOL;
                                b4:     BOOL;
                                b5:     BOOL;
                                b6:     BOOL;
                                b7:     BOOL;
                                awInt:  ARRAY[2..Range84] OF WORD;
                            END_STRUCT;
    scInternalCtrl94:         STRUCT                                  
                                b8:     BOOL:= FALSE;
                                b9:     BOOL:= FALSE;
                                b10:    BOOL:= FALSE;
                                b11:    BOOL:= FALSE;
                                b12:    BOOL:= FALSE;
                                b13:    BOOL:= FALSE;
                                b14:    BOOL:= FALSE;
                                b15:    BOOL:= FALSE;
                                b0:     BOOL:= FALSE;
                                b1:     BOOL:= FALSE;
                                b2:     BOOL:= FALSE;
                                b3:     BOOL:= FALSE;
                                b4:     BOOL:= FALSE;
                                b5:     BOOL:= FALSE;
                                b6:     BOOL:= FALSE;
                                b7:     BOOL:= FALSE;
                                awInt:  ARRAY[2..Range94] OF WORD;
                            END_STRUCT;
                            
    scInternalState94:        STRUCT                                  
                                b8:     BOOL;
                                b9:     BOOL;
                                b10:    BOOL;
                                b11:    BOOL;
                                b12:    BOOL;
                                b13:    BOOL;
                                b14:    BOOL;
                                b15:    BOOL;
                                b0:     BOOL;
                                b1:     BOOL;
                                b2:     BOOL;
                                b3:     BOOL;
                                b4:     BOOL;
                                b5:     BOOL;
                                b6:     BOOL;
                                b7:     BOOL;
                                awInt:  ARRAY[2..Range94] OF WORD;
                            END_STRUCT;
    dwTemp                      :DWORD;
    rTemp                       :REAL;
    diSetPos                    :DINT;
    
END_VAR

BEGIN

wFirstPeripherieADR:= INT_TO_WORD(iFirstPeripherieADR);             
iReadError:=0;
iWriteError:=0;

IF xEnableInternalControl THEN                                      // Interne Steuerung des Reglers
    xInternalControlActive:=True;
ELSE                                                               // Steuerung des Reglers über Eingangsvariablen des Bausteins
    xInternalControlActive:=False;
END_IF;



// keine Interne Steuerung aktiv ?
IF NOT xInternalControlActive THEN
    // Eingangssignale auf interne Variablen kopieren
    xDriveEnable_IC             := xDriveEnable;
    xDriveSetQsp_IC             := xDriveSetQsp;
    xResetError_IC              := xResetError;
    xManualPos_IC               := xManualPos;
    xManualNeg_IC               := xManualNeg;
    xHomingSet_IC               := xHomingSet;
    xHomingStart_IC             := xHomingStart;
    xHomingReset_IC             := xHomingReset;
    xProfileStart_IC            := xProfileStart;
    xProfileRestart_IC          := xProfileRestart;
    xProfileReset_IC            := xProfileReset;
    xProfilePosStop_IC          := xProfilePosStop;
    wProfileNumberSet_IC        := wProfileNumberSet;
    xEnableSpeedOverride_IC     := xEnableSpeedOverride;
    rSpeedOverrideSet_IC        := rSpeedOverrideSet;
    xEnablePositionTeach_IC     := xEnablePositionTeach;
    rPositionSet_IC             := rPositionSet;
END_IF;


CASE iDriveType OF
       
 // L-Force 8400 Highline / Topline / 8400 Protec Highline
 // Eingangsdaten vom Regler lesen
 2: iReadError:= DPRD_DAT(
                 LADDR := wFirstPeripherieADR, 
                 RECORD := scInternalState84 ); 
           
 // Statusdaten zuweisen
    xDriveError         :=scInternalState84.b0;
    xDriveEnabled       :=NOT scInternalState84.b3;
    xDriveReady         :=scInternalState84.b4;
    xHomingDone         :=scInternalState84.b6;
    xProfileDone        :=scInternalState84.b7;
    xProfileInTarget    :=scInternalState84.b8;
    xProfileActive      :=scInternalState84.b9; 
    xHomePosAvailable   :=scInternalState84.b10;
    xSpeedEqZero        :=scInternalState84.b11;
    xHW_LimitSwitchNeg  :=scInternalState84.b12;
    xHW_LimitSwitchPos  :=scInternalState84.b13;
    xDriveQspActive     :=scInternalState84.b14;
    xDriveWarning       :=scInternalState84.b15;
    
    xHomingActive       :=xHomingStart_IC AND scInternalState84.b9; 

    rSpeedActual        :=LIMIT( MN:= -199.99, IN:=(INT_TO_REAL(WORD_TO_INT(scInternalState84.awInt[2])) / 163.848), MX:=199.99);
    diActPos_p          :=WORD_TO_DINT(scInternalState84.awInt[3]) + DWORD_TO_DINT(SHL(IN:=WORD_TO_DWORD(scInternalState84.awInt[4]),N:=16)); 
    dwTemp              :=AxisData.dd72;   // AxisData.rPosToUnits
    rTemp               :=DWORD_TO_REAL(dwTemp);
    rPosActual          :=DINT_TO_REAL(diActPos_p) *  rTemp;

    IF xHomingStart_IC // Homing ?
        AND NOT xProfileStart_IC 
        AND NOT xProfileRestart_IC 
        AND NOT xManualPos_IC
        AND NOT xManualNeg_IC THEN 
    // Auswahl 1. Profil für Referenzieren
        wProfileNumberSet_IC := 1;
        xManualActive   := False;
    ELSIF ((xManualPos_IC) OR (xManualNeg_IC)) // Manual ?
        AND NOT xProfileStart_IC 
        AND NOT xProfileRestart_IC 
        AND NOT xHomingStart_IC
        AND NOT xHomingSet_IC THEN
    // Auswahl 0. Profil für Handfahren
        wProfileNumberSet_IC := 0;
        xManualActive   := True; 
    ELSE
        xManualActive   := False;
    
    END_IF;     
    
    
            
    IF (NOT xDriveEnable_IC) OR xDriveError THEN
   // Merker für Positionieren zurücksetzen
       xProfileStart_IC     := FALSE;
       xProfileRestart_IC   := FALSE;
       xProfileStart_IC     := FALSE;
       xProfileActive       := FALSE;
       xHomingStart_IC      := False;
       xHomingSet_IC        := FALSE;
       xManualPos_IC        := FALSE;
       xManualPos_IC        := FALSE;    
       // wProfileNumberSet_IC := 0;     Mit V1.1 entfallen       
                   
    END_IF;            




   // Steuerdaten zuweisen
        scInternalCtrl84.b0:=xProfilePosStop_IC;
        scInternalCtrl84.b1:=xProfileRestart_IC AND NOT xProfilePosStop_IC;
        scInternalCtrl84.b2:=xEnablePositionTeach_IC AND xProfileStart_IC;
        scInternalCtrl84.b3:=NOT xDriveEnable_IC;
        scInternalCtrl84.b4:=xwInput[8];
        scInternalCtrl84.b5:=xwInput[9];
        scInternalCtrl84.b6:=xwInput[10];
        scInternalCtrl84.b7:=xwInput[11];
        scInternalCtrl84.b8:=xProfileStart_IC OR xHomingStart_IC AND NOT xProfilePosStop_IC;
        scInternalCtrl84.b9:=xEnableSpeedOverride_IC;
        scInternalCtrl84.b10:=xHomingSet_IC;
        scInternalCtrl84.b11:=xResetError_IC;
        scInternalCtrl84.b12:=xManualNeg_IC;
        scInternalCtrl84.b13:=xManualPos_IC;
        scInternalCtrl84.b14:=xHomingReset_IC;
        scInternalCtrl84.b15:=xDriveSetQsp_IC;
        
   // limit rSpeedOverride
        rSpeedOverrideSet_IC := LIMIT(MN:=0.0, IN:=rSpeedOverrideSet_IC, MX:=199.0);
   // scale and Write rSpeedOverride to drive
        IF xEnableSpeedOverride_IC THEN
            scInternalCtrl84.awInt[2]:=INT_TO_WORD(REAL_TO_INT(rSpeedOverrideSet_IC * 163.84));
        ELSE
            scInternalCtrl84.awInt[2]:= 16384;
        END_IF;
   //    convert units to increments    
        dwTemp              :=AxisData.dd68; // DB SetAxisData.rUnitsToPos
        rTemp               :=DWORD_TO_REAL(dwTemp);
        diSetPos            :=REAL_TO_DINT(rPositionSet_IC *  rTemp) ;

   // write to drive*)
        scInternalCtrl84.awInt[3]:=DINT_TO_WORD(diSetPos);
        scInternalCtrl84.awInt[4]:=DWORD_TO_WORD(SHR(IN:=DINT_TO_DWORD(diSetPos),N:=16));
  
   // Ausgangsdaten zum Regler schreiben                         
        iWriteError:=DPWR_DAT(
                        LADDR := wFirstPeripherieADR, 
                        RECORD := scInternalCtrl84 ); 
                        
        xError:= NOT xCommunicationOK 
                 OR NOT ((iDriveType <> 2) OR (iDriveType <> 11))
                 OR (WORD_TO_INT(wProfileNumberSet_IC) > 15)
                 OR (WORD_TO_INT(wProfileNumberSet_IC) < 0);
                

// L-Force 9400 Highline 
// Eingangsdaten vom Regler lesen
11: iReadError:= DPRD_DAT(
                 LADDR := wFirstPeripherieADR, 
                 RECORD := scInternalState94 ); 
           
 // Statusdaten zuweisen
    xDriveError         :=scInternalState94.b0;
    xDriveEnabled       :=NOT scInternalState94.b1;
    xDriveReady         :=scInternalState94.b2;
    xDriveQspActive     :=scInternalState94.b3;
    xSpeedEqZero        :=scInternalState94.b4;
    xHomingActive       :=scInternalState94.b5; 
    xHomingDone         :=scInternalState94.b6;
    xHomePosAvailable   :=scInternalState94.b7;
    xManualActive       :=scInternalState94.b8;
    xDriveWarning       :=scInternalState94.b9;
    xProfileActive      :=scInternalState94.b10; 
    xProfileDone        :=scInternalState94.b11;
    xProfileInTarget    :=scInternalState94.b12;
    // xHomeSwitch      :=scInternalState94.b13;
    xHW_LimitSwitchPos  :=scInternalState94.b14;
    xHW_LimitSwitchNeg  :=scInternalState94.b15;

    rSpeedActual        :=LIMIT( MN:= -199.99, IN:=(INT_TO_REAL(WORD_TO_INT(scInternalState94.awInt[2])) / 163.848), MX:=199.99);
    diActPos_p          :=WORD_TO_DINT(scInternalState94.awInt[4]) + DWORD_TO_DINT(SHL(IN:=WORD_TO_DWORD(scInternalState94.awInt[3]),N:=16)); 
    dwTemp              :=AxisData.dd72;   // AxisData.rPosToUnits
    rTemp               :=DWORD_TO_REAL(dwTemp);
    rPosActual          :=DINT_TO_REAL(diActPos_p) *  rTemp;

     
    
            
    IF (NOT xDriveEnable_IC) OR xDriveError THEN
   // Merker für Positionieren zurücksetzen
       xProfileStart_IC     := FALSE;
       xProfileRestart_IC   := FALSE;
       xProfileStart_IC     := FALSE;
       xProfileActive       := FALSE;
       xHomingStart_IC      := False;
       xHomingSet_IC        := FALSE;
       xManualPos_IC        := FALSE;
       xManualPos_IC        := FALSE;    
       // wProfileNumberSet_IC := 1; Mit V1.1 entfallen           
    END_IF;            




   // Steuerdaten zuweisen
        scInternalCtrl94.b0:=NOT xDriveEnable_IC;
        scInternalCtrl94.b1:=xDriveSetQsp_IC;
        scInternalCtrl94.b2:=xEnablePositionTeach_IC;
        scInternalCtrl94.b3:=xHomingStart_IC;
        scInternalCtrl94.b4:=xHomingReset_IC;
        scInternalCtrl94.b5:=xProfileActive OR xProfileStart_IC;
        scInternalCtrl94.b6:=xProfileStart_IC AND NOT xProfilePosStop_IC;
        scInternalCtrl94.b7:=xProfileRestart_IC AND NOT xProfilePosStop_IC;
        scInternalCtrl94.b8:=xProfileReset_IC;
        scInternalCtrl94.b9:=xEnableSpeedOverride_IC;
        scInternalCtrl94.b10:=xResetError_IC;
        scInternalCtrl94.b11:=False; // Spare Set Error
        scInternalCtrl94.b12:=xHomingSet_IC;
        scInternalCtrl94.b13:=False; // Spare
        scInternalCtrl94.b14:=xManualPos_IC;
        scInternalCtrl94.b15:=xManualNeg_IC;
        
   // limit rSpeedOverride
        rSpeedOverrideSet_IC := LIMIT(MN:=0.0, IN:=rSpeedOverrideSet_IC, MX:=199.0);
   // scale and Write rSpeedOverride to drive
        IF xEnableSpeedOverride_IC THEN
            scInternalCtrl94.awInt[2]:=INT_TO_WORD(REAL_TO_INT(rSpeedOverrideSet_IC * 163.84));
        ELSE
            scInternalCtrl94.awInt[2]:= 16384;
        END_IF;
   // convert units to increments    
        dwTemp              :=AxisData.dd68; // DB SetAxisData.rUnitsToPos
        rTemp               :=DWORD_TO_REAL(dwTemp);
        diSetPos            :=REAL_TO_DINT(rPositionSet_IC *  rTemp) ;

   // write to drive*)
        scInternalCtrl94.awInt[4]:=DINT_TO_WORD(diSetPos);
        scInternalCtrl94.awInt[3]:=DWORD_TO_WORD(SHR(IN:=DINT_TO_DWORD(diSetPos),N:=16));
   
   // limit  ProfileNumber and write To Drive      
        scInternalCtrl94.awInt[5]:= INT_TO_WORD(LIMIT(MN:=1, IN:=WORD_TO_INT(wProfileNumberSet_IC), MX:=75));
  
   // Ausgangsdaten zum Regler schreiben                         
        iWriteError:=DPWR_DAT(
                        LADDR := wFirstPeripherieADR, 
                        RECORD := scInternalCtrl94 ); 
                        
        xError:= NOT xCommunicationOK 
                 OR NOT ((iDriveType <> 1) OR (iDriveType <> 2) OR (iDriveType <> 11))
                 OR (WORD_TO_INT(wProfileNumberSet_IC) > 75)
                 OR (WORD_TO_INT(wProfileNumberSet_IC) < 1);
  

ELSE:
    xError:= True;
END_CASE;





                                                                    
IF (iReadError <> 0) OR (iWriteError <> 0) THEN
    xCommunicationOK:=false;
ELSE
    xCommunicationOK:=True;
END_IF;

    
     
END_FUNCTION_BLOCK